#include "stmflash.h"
#include "delay.h"
#include "usart.h" 

void STMFLASH_Unlock(void)
{
	FLASH->KEYR=FLASH_KEY1;
	FLASH->KEYR=FLASH_KEY2;
}

void STMFLASH_Lock(void)
{
	FLASH->CR|=(u32)1<<31;
}

u8 STMFLASH_GetStatus(void)
{	
	 u32 res=0;		
	res=FLASH->SR;  
	if(res&(1<<16))return 1;
	else if(res&(1<<4))return 2;
	else if(res&(1<<5))return 2;
	else if(res&(1<<6))return 2;
	else if(res&(1<<7))return 2;
	return 0;
} 

u8 STMFLASH_WaitDone(u32 time)
{
	u8 res;
	do
	{
		res=STMFLASH_GetStatus();
		if(res!=1)break;
		delay_us(10);
		time--;
	 }while(time);
	 if(time==0)res=0xff;
	 return res;
}

u8 STMFLASH_EraseSector(u32 sectoraddr)
{
	u8 res=0;
	res=STMFLASH_WaitDone(200000);
	if(res==0)
	{ 
		FLASH->CR&=~(3<<8);
		FLASH->CR|=2<<8;
		FLASH->CR&=~(0X1F<<3);
		FLASH->CR|=sectoraddr<<3;
		FLASH->CR|=1<<1;
		FLASH->CR|=1<<16;
		res=STMFLASH_WaitDone(200000);
		if(res!=1)
		{
			FLASH->CR&=~(1<<1);
		}
	}
	return res;
}

u8 STMFLASH_WriteWord(u32 faddr, u32 dat)
{
	u8 res;	   	    
	res=STMFLASH_WaitDone(0XFF);	 
	if(res==0)
	{
		FLASH->CR&=~(3<<8);
		FLASH->CR|=2<<8;
 		FLASH->CR|=1<<0;
		*(vu32*)faddr=dat;
		res=STMFLASH_WaitDone(0XFF);
		if(res!=1)
		{
			FLASH->CR&=~(1<<0);
		}
	} 
	return res;
} 

u32 STMFLASH_ReadWord(u32 faddr)
{
	return *(vu32*)faddr; 
}  

u8 STMFLASH_GetFlashSector(u32 addr)
{
	if(addr<ADDR_FLASH_SECTOR_1)return 0;
	else if(addr<ADDR_FLASH_SECTOR_2)return 1;
	else if(addr<ADDR_FLASH_SECTOR_3)return 2;
	else if(addr<ADDR_FLASH_SECTOR_4)return 3;
	else if(addr<ADDR_FLASH_SECTOR_5)return 4;
	else if(addr<ADDR_FLASH_SECTOR_6)return 5;
	else if(addr<ADDR_FLASH_SECTOR_7)return 6;
	else if(addr<ADDR_FLASH_SECTOR_8)return 7;
	else if(addr<ADDR_FLASH_SECTOR_9)return 8;
	else if(addr<ADDR_FLASH_SECTOR_10)return 9;
	else if(addr<ADDR_FLASH_SECTOR_11)return 10; 
	return 11;	
}

void STMFLASH_Write(u32 WriteAddr,u32 *pBuffer,u32 NumToWrite)	
{ 
	u8 status = 0;
	u32 addrx = 0;
	u32 endaddr = 0;

  	if (WriteAddr < STM32_FLASH_BASE || WriteAddr % 4)
  	{
  		return;
  	}

	STMFLASH_Unlock();

	FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGAERR | FLASH_FLAG_WRPERR | \
		FLASH_FLAG_OPERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR | FLASH_FLAG_RDERR);

 	FLASH->ACR &= ~(1<<10);
	addrx = WriteAddr;
	endaddr = WriteAddr + NumToWrite * 4;
	if (addrx < 0X1FFF0000)
	{
		while (addrx < endaddr)
		{
			if (STMFLASH_ReadWord(addrx) != 0XFFFFFFFF)
			{   
				status = STMFLASH_EraseSector(STMFLASH_GetFlashSector(addrx));
				if (status)
					break;
			}
			else addrx+=4;
		} 
	}
	
	if(status == 0)
	{
		while(WriteAddr<endaddr)
		{
			if(STMFLASH_WriteWord(WriteAddr,*pBuffer))
			{ 
				break;
			}
			WriteAddr+=4;
			pBuffer++;
		} 
	}
	FLASH->ACR |= 1<<10;
	STMFLASH_Lock();
} 

void STMFLASH_Read(u32 ReadAddr, u32 *pBuffer, u32 NumToRead)   	
{
	u32 i;
	
	for (i = 0; i < NumToRead; i++)
	{
		pBuffer[i] = STMFLASH_ReadWord(ReadAddr);
		ReadAddr += 4;
	}
}

void Test_Write(u32 WriteAddr,u32 WriteData)   	
{
	STMFLASH_Write(WriteAddr, &WriteData, 1);
}

#if 1 /* param */
#define HQ_PARAM_FLASH_START_ADDR ADDR_FLASH_SECTOR_6

void hq_flash_param_save(u8* pData)
{
	STMFLASH_Write(HQ_PARAM_FLASH_START_ADDR, (u32*)pData, 4);
}

void hq_flash_param_get(u8* pData)
{
	STMFLASH_Read(HQ_PARAM_FLASH_START_ADDR, (u32*)pData, 4);
}
#endif

#if 1 /* sip firmware */
#define HQ_SIP_FIRMWARE_FLASH_START_ADDR	ADDR_FLASH_SECTOR_7

void hq_flash_sip_fw_one_frame_save(u8* pData, u16 len, u8 frame_no)
{
	u32 start_addr = HQ_PARAM_FLASH_START_ADDR;

	if (frame_no > 63 || !pData || len <= 0 || 0 != (len % 4))
	{
		return ;
	}

	start_addr += 2048 * frame_no;
	STMFLASH_Write(start_addr, (u32*)pData, len / 4);
}

void hq_flash_sip_fw_one_frame_get(u8* pData, u16 len, u8 frame_no)
{
	u32 start_addr = HQ_PARAM_FLASH_START_ADDR;

	if (frame_no > 63 || !pData || len <= 0 || 0 != (len % 4))
	{
		return ;
	}

	start_addr += 2048 * frame_no;

	STMFLASH_Read(start_addr, (u32*)pData, len / 4);
}
#endif

